#ifndef FACTOR_H
#define FACTOR_H
#include <sophus/se3.hpp>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/base_binary_edge.h>


class node : public g2o::BaseVertex<6, Sophus::SE3d> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    node(const Sophus::SE3d& pose);
    node(){}
    virtual ~node() {}
    bool read ( std::istream& ) {
        return true;
    }

    bool write ( std::ostream&) const {
        return true;
    }

    virtual void setToOriginImpl() {
        _estimate = Sophus::SE3d();
    }

protected:
    virtual void oplusImpl(const double* v);
};

class point : public g2o::BaseVertex<3, Eigen::Vector3d> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    point() {}
    point(Eigen::Vector3d &point_) {_estimate = point_;}
    virtual ~point(){}
    bool read ( std::istream& ) {
        return true;
    }

    bool write ( std::ostream&) const {
        return true;
    }

    virtual void setToOriginImpl() {
        _estimate = Eigen::Vector3d::Zero();
    }

protected:
    virtual void oplusImpl(const double* v);

} ;

class BAFactor : public g2o::BaseBinaryEdge<2, Eigen::Vector3d, node, point> {
public:
    BAFactor(const Eigen::Vector2d &px, Eigen::Matrix<double, 2, 2>&info, Eigen::Matrix3d& K);
    virtual ~BAFactor() {}
    virtual void computeError();
    virtual void linearizeOplus();
    virtual bool read(std::istream&) {return true;}
    virtual bool write(std::ostream&) const {return true;}

public:
    Eigen::Vector3d P;
    double sin;
    double cos;
    double cos2;
    double a, b, c;
    double norm, dot;
};

class unary_gps_factor : public g2o::BaseUnaryEdge<6, Sophus::SE3d, node> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    unary_gps_factor(const Sophus::SE3d& T, Eigen::Matrix<double, 6, 6>&info);
    virtual ~unary_gps_factor(){}
    virtual void computeError();
    virtual void linearizeOplus();
    virtual bool read(std::istream&) {return true;}
    virtual bool write(std::ostream&) const {return true;}
};

class binary_gps_factor : public g2o::BaseBinaryEdge<6, Sophus::SE3d, node, node> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    binary_gps_factor(const Sophus::SE3d& T, Eigen::Matrix<double, 6, 6>&info, double alpha);
    virtual ~binary_gps_factor(){}
    virtual void computeError();
    virtual void linearizeOplus();
    virtual bool read(std::istream&) {return true;}
    virtual bool write(std::ostream&) const {return true;}

private:
    double _alpha;
};

#endif // FACTOR_H
